﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest.SelfLocators.MBL
{
  class MBLSimpleGrid : SelfLocator
  {
    List<FunctionWorldState> functionTemplate;
    bool functionInitialized = false;
    Type currentWorldStateType = null;

    public void InitFunction(WorldState sampleWorldState)
    {
      currentWorldStateType = sampleWorldState.GetType();
      functionInitialized = true;
      functionTemplate = sampleWorldState.CreateListOfWorldStates(
        Globals.GetMinimum(sampleWorldState),
        Globals.GetMaximum(sampleWorldState),
        Globals.GetStep(sampleWorldState));
    }

    public override WorldState Execute(Experiment experiment)
    {
      if (!functionInitialized || experiment.simulatedWorldState.GetType() != currentWorldStateType) InitFunction(experiment.simulatedWorldState);
      
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);
      if (listOfOldPlaces.Count == 0) return null;

      if (DebugKeyTool.IsActive("MBLSimpleGrid:heinrichs test"))
      {
        int n = 3;
        int idx = 0;
        ActionAndObservationHistory.RelativePlaceWithMeasurement[] placeList = new ActionAndObservationHistory.RelativePlaceWithMeasurement[n];
        foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation in listOfOldPlaces)
        {
          if (worldStateAndObservation.measurement != null)
          {
            placeList[idx++] = worldStateAndObservation;
            if (idx == n)
              break;
          }//end if
        }//end for

        /*
      WorldState info = (WorldState)System.Activator.CreateInstance(experiment.simulatedWorldState.GetType());
      ActionAndObservationHistory.RelativePlacesInfo placeInfo1 = 
         new ActionAndObservationHistory.RelativePlacesInfo(info, experiment.sensors[0].PredictPerfcet(info, experiment.map),0);
      ActionAndObservationHistory.RelativePlacesInfo placeInfo2 =
     new ActionAndObservationHistory.RelativePlacesInfo(info, experiment.sensors[1].PredictPerfcet(info, experiment.map), 0);
      ActionAndObservationHistory.RelativePlacesInfo placeInfo3 =
     new ActionAndObservationHistory.RelativePlacesInfo(info, experiment.sensors[2].PredictPerfcet(info, experiment.map), 0);
        */

        if (idx == n)
        {
          List<FunctionWorldState> myNewFunction = new List<FunctionWorldState>();
          for (int i = 0; i < functionTemplate.Count; i++)
          {
            FunctionWorldState entry = functionTemplate[i];
            entry.value = MBLTools.evaluateLM(entry.worldState, placeList, experiment);
            myNewFunction.Add(entry);
          }//end foreach
          DrawFunction("world:MBLSimpleGrid:mein test", myNewFunction, 0, 2000);
        }//end if
      }

      double minimalValue;
      double maximalValue;
      WorldState positionOfMinimalValue;
      WorldState positionOfMaximalValue;

      List<FunctionWorldState> newFunction;

      FindMinimum(listOfOldPlaces, 
        functionTemplate, out newFunction,
        out minimalValue, out positionOfMinimalValue,
        out maximalValue, out positionOfMaximalValue,
        experiment);

      DrawFunction("world:MBLSimpleGrid:likelihood function", newFunction, minimalValue, maximalValue);

      return positionOfMinimalValue;
    }

    private void FindMinimum(
    List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces,
    List<FunctionWorldState> functionTemplate,
    out List<FunctionWorldState> newFunction,
    out double minimalValue,
    out WorldState positionOfMinimalValue,
    out double maximalValue,
    out WorldState positionOfMaximalValue,
    Experiment setup)
    {
      maximalValue = Double.MinValue;
      minimalValue = Double.MaxValue;

      positionOfMinimalValue = null;
      positionOfMaximalValue = null;

      newFunction = new List<FunctionWorldState>();
      for (int index = 0; index < functionTemplate.Count; index++)
      {
        FunctionWorldState entry = functionTemplate[index];
        double value = MBLTools.CalculateFunction2(listOfOldPlaces, entry.worldState, setup);
        newFunction.Add(new FunctionWorldState(entry.worldState, value));
        if (value > maximalValue)
        {
          maximalValue = value;
          positionOfMaximalValue = entry.worldState;
        }
        if (value < minimalValue)
        {
          minimalValue = value;
          positionOfMinimalValue = entry.worldState;
        }
      }
    }

    private static void DrawFunction(string drawingID,
      List<FunctionWorldState> function, double minimalValue, double maximalValue)
    {
      if (maximalValue - minimalValue == 0) return;
      foreach (FunctionWorldState entry in function)
      {
        int intensity = (int)(entry.value / maximalValue * 255);
        entry.worldState.DrawWithIntensity(intensity, drawingID, Globals.rasterWidth);
      }
    }

  }
}
